/*
 * MPU_6050.cpp
 *
 *      Author: Christian
 *      Letztes Update: 21.02.14 Christian | Code Aufräumen
 *
 */

#include "MPU_6050.h"

int16_t xA_off;
int16_t yA_off;
int16_t zA_off;

int16_t xG_off;
int16_t yG_off;
int16_t zG_off;

static int16_t mpu_offset(uint8_t reg) {
	int16_t tmp = 0;
	int32_t tmp2 = 0;
	for (uint8_t i = 0; i < 200; i++) {

		mpu_read_value(reg, &tmp);
		tmp2 = tmp2 + tmp;
		_delay_us(200);
	}
	return (tmp2 / 200);
}

void mpu_init(void) {

	twi_init();

	_delay_ms(10);

// Sample Rate Divider einstellen
	mpu_write_register( SMPLRT_DIV, 0x00);

// EXT-SYNC aus Low-pass-Filter auf Stufe 2
	mpu_write_register( CONFIG, 0x04);

// Gyro-Config
	mpu_write_register( GYRO_CONFIG, GYRO_NO_SELF_TEST_2000); // Self-Test aus, +-2000grad/s Range

//Accel-Config
	mpu_write_register( ACCEL_CONFIG, ACCEL_NO_SELF_TEST_4); // Self-Test aus, +-4g Range

// SLEEP Modus ausschalten

	mpu_write_register( PWR_MGMT_1, 0x00);

// warten bis gyro eingeschwungen
	_delay_ms(100);

// MOT_THR und FIFO_EN bleiben 0x00
// INT_PIN_CFG und INT_ENABLE bleiben 0x00

// CLock source auf Gyro Z setzen
	mpu_write_register( PWR_MGMT_1, 0x03);
	_delay_ms(100);

	xA_off = mpu_offset( ACCEL_XOUT_H);
	yA_off = mpu_offset( ACCEL_YOUT_H);
	zA_off = mpu_offset( ACCEL_ZOUT_H);

	xG_off = mpu_offset( GYRO_XOUT_H);
	yG_off = mpu_offset( GYRO_YOUT_H);
	zG_off = mpu_offset( GYRO_ZOUT_H);

	mpu_ready();
}

uint8_t mpu_bias_ok() {

	int16_t temp;
	for (uint8_t i = 0; i < 10; i++) {
		mpu_read_value( GYRO_XOUT_H, &temp);
		if (abs(temp - xG_off) > KALIBRIERUNGSSCHWELLE) {
			return 0;
		}
		mpu_read_value( GYRO_YOUT_H, &temp);
		if (abs(temp - yG_off) > KALIBRIERUNGSSCHWELLE) {
			return 0;
		}
		mpu_read_value( GYRO_ZOUT_H, &temp);
		if (abs(temp - zG_off) > KALIBRIERUNGSSCHWELLE) {
			return 0;
		}

		mpu_read_value( ACCEL_XOUT_H, &temp);
		if (abs(temp - xA_off) > KALIBRIERUNGSSCHWELLE) {
			return 0;
		}

		mpu_read_value( ACCEL_YOUT_H, &temp);
		if (abs(temp - yA_off) > KALIBRIERUNGSSCHWELLE) {
			return 0;
		}

		mpu_read_value( ACCEL_ZOUT_H, &temp);
		if (abs(temp - zA_off) > KALIBRIERUNGSSCHWELLE) {
			return 0;
		}
	}

	return 1;

}

uint8_t mpu_self_test(void) {

	/*	uint16_t temp = 0;
	 uint16_t str[3] = { 0, 0, 0 };
	 uint16_t ft[3] = { 0, 0, 0 };
	 uint8_t xyz_test[3] = { 0, 0, 0};

	 // Accelerometer:


	 // Gyro:

	 // STR Bestimmen

	 for ( uint8_t i = 0; i < 3; i++)
	 {
	 mpu_read_value( GYRO_XOUT_H + ( i * 0x02 ) , &temp );
	 str[i] = temp;
	 }

	 mpu_write_register( GYRO_CONFIG, GYRO_SELF_TEST_CONFIG );

	 for ( uint8_t i = 0; i < 3; i++)
	 {
	 mpu_read_value( GYRO_XOUT_H + ( i * 0x02 ) , &temp );
	 str[i] = temp - str[i];
	 }

	 //xyz_test f�r gyro bestimmen
	 for ( uint8_t i = 0; i < 3; i++)
	 {
	 mpu_read_register( SELF_TEST_X + ( i * 0x01 ), xyz_test + i);
	 xyz_test[i] &= 0x1F;
	 }

	 //FT berechnen


	 */
	return 1;
}

uint8_t mpu_write_register(uint8_t reg, uint8_t wert) {

	if (twi_start( MPU_6050_SLAVE_ADDRESS_W)) {
		if (twi_send_data(reg)) {
			if (twi_send_data(wert)) {
				twi_stop();
				return 1;
			}
		}
	}
	i2c_error();
	return 0;
}

uint8_t mpu_read_register(uint8_t reg, uint8_t *data) {

	if (twi_start( MPU_6050_SLAVE_ADDRESS_W)) {
		if (twi_send_data(reg)) {
			if (twi_repstart( MPU_6050_SLAVE_ADDRESS_R)) {
				*data = twi_recieve_data_nack();
				twi_stop();
				return 1;
			}
		}
	}
	i2c_error();
	return 0;
}

uint8_t mpu_read_value(uint8_t reg_h, int16_t *data) {

	int16_t* temp = 0;
	if (twi_start( MPU_6050_SLAVE_ADDRESS_W)) {
		if (twi_send_data(reg_h)) {
			if (twi_repstart( MPU_6050_SLAVE_ADDRESS_R)) {
				*data = twi_recieve_data_ack();
				*data = *data << 8;
				*temp = twi_recieve_data_nack();
				*data |= *temp;
				twi_stop();
				return 1;
			}
		}
	}
	i2c_error();
	return 0;
}

